#include "Detector.h"
#include "arm_math.h"

void DetectorInit(Detector *d, float StaTime, float LtaTime, float freq) {
	d->alphaSTA = 2.0f / (StaTime * freq + 1.0f);
	d->alphaLTA = 2.0f / (LtaTime * freq + 1.0f);
	d->alpha06Thr = 2.0f / (0.6f * freq + 1.0f);
  d->alpha02Thr = 2.0f / (0.2f * freq + 1.0f);
	d->oneMinusAlphaSTA = 1.0f - d->alphaSTA;
	d->oneMinusAlphaLTA = 1.0f - d->alphaLTA;
	d->oneMinusAlpha06Thr = 1.0f - d->alpha06Thr;
	d->oneMinusAlpha02Thr = 1.0f - d->alpha02Thr;
	d->begFound = false;
}

float STALTA(Detector *d, short sample) {
	d->LTAValue = d->alphaLTA * sample + d->oneMinusAlphaLTA * d->LTAValue; 
	d->STAValue = d->alphaSTA * sample + d->oneMinusAlphaSTA * d->STAValue;
	if (d->LTAValue == 0) return 0;
	return d->STAValue / d->LTAValue;
}

float LTA(Detector *d, short sample) { 
	d->LTAValue = d->alphaLTA * sample + d->oneMinusAlphaLTA * d->LTAValue; 
	return d->LTAValue;
}

float STA(Detector *d, short sample) {
	d->STAValue = d->alphaSTA * sample + d->oneMinusAlphaSTA * d->STAValue;
	return d->STAValue;
}

float Threshold(Detector *d, short sample) {
	d->EMA06 = d->alpha06Thr * sample + d->oneMinusAlpha06Thr * d->EMA06;
	d->EMA02 = d->alpha02Thr * sample + d->oneMinusAlpha02Thr * d->EMA02;
	return (d->EMA06 + d->EMA02 * 0.05f) + 1.0f;
}

float CS(Detector *d, float stalta) {
	d->cs = log(stalta * 0.8f) + d->cs;
	return d->cs;
}
